/**

Title   : OmniWidget.cpp
Author  : Manikantan Nambi
URL :

Description : Functions for using the Phantom Omni haptic device
Created : 9/6/11
Modified   :
*/

#include "OmniWidget.h"

int countCall;
extern double velmag;
extern double veldir;

/*
*   Summary:   OmniThread constructor
*
*   Parameters:     parent widget
*   Returns:    None
*/
OmniWidget::OmniWidget(QObject *parent) : QThread(parent) {
    qDebug()<< "\nOmniWidget::OmniWidget(): Initializing the Phantom Omni ";

    countRun = 0;     // initializing variables
    countCall = 0;

    state = new DeviceDisplayState(); // create new object to store the state of the omni
    state->planeStiffness = 0.5; // define virtual horizontal plane stiffness N/mm
    state->mm3aSpringStiffness = 0.1; // define anchor stiffness used for velocity control

    // Initialize the default haptic device.
    HDErrorInfo error;
    state->dHandle = hdInitDevice(HD_DEFAULT_DEVICE); // initial Phantom Omni

    if (HD_DEVICE_ERROR(error = hdGetError()))  // check for errors
    {
        hduPrintError(stderr, &error, "Failed to initialize haptic device");
        omniConnected = 0;
    } else {
        omniConnected = 1;

        hdMakeCurrentDevice(state->dHandle); // make device current

        if(hdCheckCalibration()==HD_CALIBRATION_NEEDS_UPDATE) {  // check if calibration is needed
            qDebug()<<"OmniWidget::OmniWidget(): Calibration needs to be updated";
            hdUpdateCalibration(HD_CALIBRATION_INKWELL);
        } else {
            qDebug()<<"OmniWidget::OmniWidget(): Device is calibrated";
        }
    }
}


/*
*   Summary:  Executes when then thread is started. Sets up the synchronos calling function for the Omni
*
*   Parameters:     parent widget
*   Returns:    None
*/
void OmniWidget::run()
{
    qDebug()<<"OmniThread:run(): Starting scheduler to poll data from Omni";
    hdScheduleSynchronous(DeviceStateCallback, state, HD_MIN_SCHEDULER_PRIORITY); // start scheduler
    exec();
}


/*
*   Summary:  Enables force output for the Omni which is disabled by default, starts the scheduler, and
*                        obtains y co-ordinate of the virtual plane.
*
*   Parameters:     parent widget
*   Returns:    None
*/

void OmniWidget::open()
{
    hdEnable(HD_FORCE_OUTPUT);  // enable force output to device
    hdStartScheduler();     // start scheduler

    hdBeginFrame(state->dHandle);
    hdGetDoublev(HD_CURRENT_POSITION, state->pos);
    hdEndFrame(state->dHandle);
    state->planeY = state->pos[1]; // store initial y co-ordinate of stylus as the plane
    qDebug()<< "OmniThread::open():PlaneY for virtual plane = "<< state->planeY;
}

/*
*   Summary:  Disables force output and stops the scheduler.
*
*   Parameters:     parent widget
*   Returns:    None
*/
void OmniWidget::close()
{
    hdDisable(HD_FORCE_OUTPUT);  // disable force output to the device
    hdStopScheduler(); // stop scheduler
    qDebug()<<"Closing device Omni";
}

/*
*   Summary:  Callback function for the Omni which is called for each tick of the scheduler. All the haptics calculations are
*                      are performed in this function.
*   Parameters:     parent widget
*   Returns:    None
*/

HDCallbackCode HDCALLBACK DeviceStateCallback(void *pUserData)
{
    DeviceDisplayState *pDisplayState = static_cast<DeviceDisplayState *>(pUserData);
    countCall++; // increment counter

    HDint nCurrentButtons, nLastButtons;
    nCurrentButtons = pDisplayState->nCurrentButtons; // get current button state
    nLastButtons = pDisplayState->nLastButtons; // get last button state

    // check if button is pressed in this cycle
    hdBeginFrame(pDisplayState->dHandle);
    hdGetDoublev(HD_CURRENT_POSITION, pDisplayState->pos);
    hdGetIntegerv(HD_CURRENT_BUTTONS, &pDisplayState->nCurrentButtons);
    hdGetIntegerv(HD_LAST_BUTTONS, &pDisplayState->nLastButtons);


    //had made modifications for Jake and Dr. Ambatti to find stiffness during cataract surgery
    if ((nCurrentButtons & HD_DEVICE_BUTTON_1) != 0 && (nLastButtons & HD_DEVICE_BUTTON_1) == 0) {
        pDisplayState->planeStiffness = pDisplayState->planeStiffness + 0.1;
    }

    if ((nCurrentButtons & HD_DEVICE_BUTTON_2) != 0 && (nLastButtons & HD_DEVICE_BUTTON_2) == 0) {
        pDisplayState->planeStiffness = pDisplayState->planeStiffness - 0.1;
    }


    //     if(pDisplayState->planeStiffness<0.0)
    //         pDisplayState->planeStiffness = 0.0;



    // finding anchor point // commented temp
    //    if ((nCurrentButtons & HD_DEVICE_BUTTON_1) != 0 && (nLastButtons & HD_DEVICE_BUTTON_1) == 0)   {
    //        /* Detected button down */
    //        memcpy(pDisplayState->anchor, pDisplayState->pos, sizeof(HDdouble[3]));
    //        pDisplayState-> renderForce = 1; // render forces to device if button is pressed
    //    }
    //    else if ((nCurrentButtons & HD_DEVICE_BUTTON_1) == 0 &&
    //             (nLastButtons & HD_DEVICE_BUTTON_1) != 0)
    //    {
    //        pDisplayState->renderForce = 0;

    //        pDisplayState->force[0] = 0;  // Send zero force to the device, or else it will just continue rendering the last force sent
    //        pDisplayState->force[2] = 0;
    //    }s

    if(pDisplayState->renderForce) {
        // creating a force vector from anchor point to the current position of the Omni
        pDisplayState->mm3aSpringLength[0]  =  pDisplayState->pos[0] - pDisplayState->anchor[0];
        pDisplayState->mm3aSpringLength[1] = 0.0;
        pDisplayState->mm3aSpringLength[2]  =  pDisplayState->pos[2] - pDisplayState->anchor[2];
        pDisplayState->force[0] = -pDisplayState->mm3aSpringLength[0] * pDisplayState->mm3aSpringStiffness;
        pDisplayState->force[2] = -pDisplayState->mm3aSpringLength[2] * pDisplayState->mm3aSpringStiffness;

        velmag = pDisplayState->mm3aSpringLength.magnitude(); // velocity magnitude which is equal to spring length
        veldir = atan2(pDisplayState->mm3aSpringLength[2], pDisplayState->mm3aSpringLength[0]);
        //qDebug() << "Vel Mag from Omnithread = "<< pDisplayState->mm3aSpringLength.magnitude();
    } else {
        velmag = 0.0;
    }

    // creating a force vector repelling the user from the virtual plane proportional
    // to the penetration distance, using F=kx, where k is the plane stiffness and x is
    // the peneration vector.
    if(pDisplayState->pos[0]<0.0)
        pDisplayState->penetrationDistance = 0.0 - pDisplayState->pos[0];
    else
        pDisplayState->penetrationDistance =0.0;

    pDisplayState->force[0] = pDisplayState->planeStiffness * pDisplayState->penetrationDistance;
    if(pDisplayState->force[0]>0.6)
        pDisplayState->force[0]=0.6;

    qDebug()<< "Spring stiffness ="<< pDisplayState->planeStiffness << " Force =" << pDisplayState->force[0];

    hdSetDoublev(HD_CURRENT_FORCE, pDisplayState->force); // render forces


    // qDebug("%s = %2.2f %s, %s = %2.2f %s, %s  = %2.2f %s,", "Force",pDisplayState->force[1]," N","Spring Stiffness ", pDisplayState->planeStiffness," N/mm", " Depth ",pDisplayState->penetrationDistance, " mm");

    hdEndFrame(pDisplayState->dHandle);

    return HD_CALLBACK_CONTINUE;
}

